//============================================================================
// Name        : Connection.cpp
// Author      : O.M.A.V.
// Description : Implementation file van de connection class
//============================================================================

#include "Connection.h"
#include "Vn100/Vn100Device.h"
#include "motorcontroller/motorcontroller.h"
#include "log/log.h"

// Function prototypes
void HandleTCPClient(TCPSocket *sock);
void *ThreadMain(void *clientSocket);
void *Vn100Thread(void *clientSocket);

// Fields
const int RCVBUFSIZE = 1024;
bool receiveVN100;
MotorController *controller;

/*
 * Create a new connection.
 */
Connection::Connection(int port)
{
	receiveVN100 = false;
	TCPSocket serverSocket(port, 5);
	controller = new MotorController();

	while(1)
	{
		// Create seperate memory for client argument
		TCPSocket *clientSocket = serverSocket.accept();

		pthread_t threadID;
		if (pthread_create(&threadID, NULL, ThreadMain, (void *) clientSocket) != 0)
		{
			cout << "Kan geen thread maken." << endl;
			exit(1);
		}
	}
}

Connection::~Connection()
{
	delete controller;
}

/*
 * TCP Client handling function.
 */
void HandleTCPClient(TCPSocket *sock)
{

	// Receive information and echo it back
	char echoBuffer[RCVBUFSIZE];
	int rcvSize;
	while ((rcvSize = sock->recv(echoBuffer, RCVBUFSIZE)) > 0) {
		string buf(echoBuffer);
		string data = buf.substr(0, 3);

		// BESTURING - PROTOCOL
		// stop
		if (data == "101")
		{
			controller->Stop();
		}
		// vooruit
		else if (data == "102")
		{
			controller->MoveForward();
		}
		// achteruit
		else if (data == "103")
		{
			controller->MoveBackward();
		}
		// Links
		else if (data == "104")
		{
			controller->GoLeft();
		}
		// Rechts
		else if (data == "105")
		{
			controller->GoRight();
		}
		// VN100 - PROTOCOL //
		// start
		else if (data == "201")
		{
			receiveVN100 = true;

			pthread_t thread;
			if (pthread_create(&thread, NULL, Vn100Thread, sock) != 0)
			{
				cout << "Kan geen thread maken." << endl;
				exit(1);
			}
		}
		// Stop
		else if (data == "202")
		{
			receiveVN100 = false;
		}

		memset(echoBuffer, 0, strlen(echoBuffer));
	}
	receiveVN100 = false;
}

/*
 * Socket receive entry point.
 */
void *ThreadMain(void *clientSocket)
{
	// Deallocate the thread
	pthread_detach(pthread_self());
	cout << endl << "Client verbonden" << endl;

	// Extract socket descriptor from the argument
	HandleTCPClient((TCPSocket *)clientSocket);
	receiveVN100 = false;
	cout << endl << "Verbinding met client verbroken" << endl;

	// Deallocate the socket
	delete (TCPSocket *)clientSocket;

	// Exit the thread
	return NULL;
}

/*
 * Thread to send vn100 data to client
 */
void *Vn100Thread(void *clientSocket)
{
	pthread_detach(pthread_self());

	// Connect to the VN100
	Vn100Device *vn100 = new Vn100Device();

	while(receiveVN100)
	{
		TCPSocket* sock = (TCPSocket *)clientSocket;

		// Get the VN100's orientation
		VnYpr orientation = vn100->GetOrientation();

		// Format the orientation
		char msg[100];
		sprintf(msg, "%+#7.2f:%+#7.2f:%+#7.2f", orientation.yaw, orientation.pitch, orientation.roll);

		// Send the orientation
		sock->send(msg);
		usleep(100000);
	}

	// Disconnect from the VN100
	delete vn100;

	// Exit the thread
	return NULL;
}
